#include "cpp_co_location_client/UKF.hpp"
#include "cmath"

const Eigen::MatrixXd default_P = Eigen::MatrixXd::Identity(10,10) * Initial_P_sigma;

#define ConvertMsToSecond(a)\
		{(a)*float(0.001)}


float Polar_angle_cal(float x, float y) {
	float PI = 3.1415926;
	float temp_tangle = 0.0;
	if (x == 0 && y == 0) {
		temp_tangle = 0;
	} else if (y >= 0) {
		temp_tangle = (float) atan2(y, x);
	} else if (y <= 0) {
		temp_tangle = (float) atan2(y, x) + 2 * PI;
	}
	return temp_tangle;
}

float normalangle(float angle)
{
  const auto times = std::round(std::fabs(angle / (2. * M_PI)));  // for the case when angle is very very large

  if (angle > M_PI)
  {
    angle -= times * 2.0 * M_PI;
  }

  if (angle < -M_PI)
  {
    angle += times * 2.0 * M_PI;
  }
  return angle;
}

Eigen::MatrixXd sqrtm(const Eigen::MatrixXd& A) {
    // 对矩阵进行Schur分解
    Eigen::RealSchur<Eigen::MatrixXd> schur(A);
    if (schur.info() != Eigen::Success) {
        throw std::runtime_error("Schur 分解失败");
    }

    Eigen::MatrixXd T = schur.matrixT(); // 上三角矩阵
    Eigen::MatrixXd U = schur.matrixU(); // 单位正交矩阵

    // 对上三角矩阵T的对角元素开方
    for (int i = 0; i < T.rows(); ++i) {
        T(i, i) = std::sqrt(T(i, i));
    }

    // 恢复平方根矩阵
    Eigen::MatrixXd sqrt_A = U * T * U.transpose();

    return sqrt_A;
}

/**
 * @brief UKF发散或目标丢失时间超过限度后，
 * 		丢弃所有已有数据，初始化状态重置
 */
bool UKF::TargetLost()
{
	return true;
}

Eigen::VectorXd UKF::Get_state()
{
	return UKF_Output_.State;
}

Eigen::MatrixXd UKF::Get_covariance()
{
	return UKF_Output_.P;
}

/**
 * @brief UKF 构造函数
 *
 * 根据给定的参数初始化 UKF 对象。
 *
 * @param smodel 运动模型名称
 * @param state_n 状态变量数量
 * @param mea_n 测量变量数量
 * @param Q 过程噪声协方差矩阵
 * @param R 测量噪声协方差矩阵
 * @param P 初始状态协方差矩阵
 */
UKF::UKF(std::string smodel, Eigen::MatrixXd Q, int state_n, float alpha , float beta){

	if(model_hash_.count(smodel))
	model_ = model_hash_[smodel];

	n_x_ = state_n;

	UKF_Output_.State = Eigen::VectorXd(n_x_);
	UKF_Output_.State.fill(0.0);
	
	UKF_Output_.P = Eigen::MatrixXd(n_x_, n_x_);

	Q_ = Q;

	alpha_ = alpha;

	beta_ = beta;

	GenerateSigmaWeights_(mean_weights_, cov_weights_, lambda_,n_x_,alpha_,beta_);
}

// 重写
// UKF发散或目标丢失时间超过限度后，记得将is_initalized_置false，
// void UKF::Initialization(Filtered_Target_State Initial_State)
// {
// 	UKF_Output_ = Initial_State;

// 	is_initalized_ = true;

// 	// return true;
// }

void UKF::Initialization(Filtered_Target_State Initial_State, Eigen::MatrixXd Q, float alpha, float beta)
{
	UKF_Output_ = Initial_State;

	
	// std::cout<<"UKF initialized state:"<<UKF_Output_.State<<std::endl;

	Q_ = Q;

	alpha_ = alpha;

	beta_ = beta;

	likelihood_ = 1/3;


	GenerateSigmaWeights_(mean_weights_, cov_weights_, lambda_,n_x_,alpha,beta);
	
	is_initalized_ = true;
}


bool UKF::Isinitalized(){
	return is_initalized_;
}


//// 多步量测处理
// 第一步输入IMM重初始化的初始X P
// 最后一步生成并输出结果到IMM
// 中间调用UKF_Update
// 输出滤波后的X，P和时间，S，测量误差用于模型融合
bool UKF::Process(Filtered_Target_State &UKF_Output, double &likelihood, 
				std::vector<MeasureMents_In_Period> UKF_Inputs,
				Eigen::VectorXd X, Eigen::MatrixXd P)
{
	if(Isinitalized())
	{
		std::cout<<"UKF process strat"<<std::endl;
		UKF_Output_.State = X;
		UKF_Output_.P = P;
		// std::cout<<"UKF Input size is:"<<UKF_Inputs.size()<<std::endl;

		for(auto iter=UKF_Inputs.begin();iter!=UKF_Inputs.end();iter++)
		{
			// std::cout<<"UKF Input measurement time is:"<<iter->meanTime<<std::endl;
			MeasureMents_In_Period UKF_Input = *iter;

			// if(iter==UKF_Inputs.end()-1)
			// {
				// 最后一步需输出S Zminus likelihood
				if(!(UKF_Update(UKF_Output, likelihood, UKF_Input)))
				{
					std::cout<<"UKF process not good!"<<std::endl;
					is_initalized_ = false;
					return false;
				}
				else
				{
					return true;
				}
			// }
			// else{
			// 	if(!UKF_Update(UKF_Input))
			// 	{
			// 		return false;
			// 	}
			// }
		}
	}

	std::cout<<"UKF not initialized"<<std::endl;
	return false;
} 


// bool UKF::GetFilteredState(Filtered_Target_State &UKF_Output)
// {

// }


/**
 * ********************* private ************************************
 * 
 */


// /**
//  * @brief 根据UKF单步输入执行更新
//  * 		单步计算不做融合，也无需计算S Zminus likelihood
//  */
// bool UKF::UKF_Update(MeasureMents_In_Period UKF_Input)
// {
// 	UKF_Input_ = UKF_Input;
// 	// std::cout<<"UKF update strat:"<<UKF_Input.MeasurementsVector<<std::endl;
// 	// std::cout<<"UKF strat state:"<<UKF_Output_.State<<std::endl;
// 	// std::cout<<"UKF P:"<<UKF_Output_.P<<std::endl;
// 	sigma_points_ =  MakeSigmaPoints();
// 	// std::cout<<"sigma_points_:"<<sigma_points_<<std::endl;
// 	bool success;
// 	success = UpdateKinematicMatrix_(KinematicMatrix_,UKF_Input_.meanTime, UKF_Output_.FilterTime,model_);
// 	if (success)
// 	{
// 		// std::cout<<"KinematicMatrix_:"<<KinematicMatrix_<<std::endl;
// 		Eigen::VectorXd X_hat;
// 		Eigen::MatrixXd P_hat;
// 		success = Prediction(X_hat,P_hat, sigma_points_pre_,sigma_points_,KinematicMatrix_,n_x_,Q_,mean_weights_,cov_weights_);
// 		if(success)
// 		{
// 			// std::cout<<"next state predicted:"<<X_hat<<std::endl;
// 			Eigen::VectorXd mean_Measurements_hat;
// 			Eigen::MatrixXd Sigma_Measurements_hat;
// 			success = PredictionZ_hat(mean_Measurements_hat, Sigma_Measurements_hat,mean_weights_,sigma_points_pre_,
// 										UKF_Input_.MeasurementsMatrix, UKF_Input_.Airplane_Z);
// 			if(success)
// 			{
// 				// std::cout<<"measurement predicted"<<mean_Measurements_hat<<std::endl;
// 				Eigen::VectorXd State;
// 				Eigen::MatrixXd P;	
// 				success =  UpdateState( State, P, S_, Zminus_,
// 										Sigma_Measurements_hat , mean_Measurements_hat,
// 										X_hat, P_hat,
// 										sigma_points_pre_,UKF_Input_.MeasurementsVector,n_x_, 
// 										UKF_Input_.R_merge, cov_weights_);
// 				if(success)
// 				{
// 					std::cout<<"state filtered:"<<State<<std::endl;
// 					// 更新所有输出量
// 					UKF_Output_.State = State;
// 					UKF_Output_.P = P;
// 					// std::cout<<"ukf P:"<<P<<std::endl;
// 					UKF_Output_.FilterTime = UKF_Input.meanTime;
// 					return true;
// 				}
// 			}
// 		}
// 		return false;
// 	}
// 	else
// 	{
// 		return true;
// 	}
// }

/**
 * @brief 异步UKF下的最后一个量测集所带来的更新
 *     最后一步需计算S Zminus likelihood并输入IMM融合
 */
bool UKF::UKF_Update(Filtered_Target_State &output, double &likelihood_output, MeasureMents_In_Period UKF_Input)
{
	UKF_Input_ = UKF_Input;
	sigma_points_ =  MakeSigmaPoints();
	// for(int i=0; i<2*n_x_+1; ++i)
	// {
	// 	std::cout<<"col:"<<i<<"sigma_points_:"<<sigma_points_.col(i)<<std::endl;
	// }
	
	bool success;
	success = UpdateKinematicMatrix_(KinematicMatrix_,UKF_Input_.meanTime, UKF_Output_.FilterTime, model_);
	if (success)
	{
		// std::cout<<"KinematicMatrix_:"<<KinematicMatrix_<<std::endl;
		Eigen::VectorXd X_hat;
		Eigen::MatrixXd P_hat;
		success = Prediction(X_hat,P_hat, sigma_points_pre_,sigma_points_,KinematicMatrix_,n_x_,Q_,mean_weights_,cov_weights_);
		if(success)
		{
			// std::cout<<"sigma_points_pre_:"<<sigma_points_pre_<<std::endl;
			Eigen::VectorXd mean_Measurements_hat;
			Eigen::MatrixXd Sigma_Measurements_hat;
			success = PredictionZ_hat(mean_Measurements_hat, Sigma_Measurements_hat,mean_weights_,sigma_points_pre_,
										UKF_Input_.MeasurementsMatrix, UKF_Input_.Airplane_Z);
			if(success)
			{

				Eigen::VectorXd State;
				Eigen::MatrixXd P;	
				double likelihood = -1;
				State.fill(0.0);
				P.fill(0.0);
				// std::cout<<"measurement predicted"<<mean_Measurements_hat<<std::endl;
				success = UpdateState( State, P, S_, Zminus_, likelihood,
										Sigma_Measurements_hat , mean_Measurements_hat,
										X_hat, P_hat,
										sigma_points_pre_,UKF_Input_.MeasurementsVector,n_x_, 
										UKF_Input_.R_merge, cov_weights_);

				if(success)
				{
					std::cout<<"state filtered:"<<State<<std::endl;
					// 更新所有输出量
					UKF_Output_.State = State;
					UKF_Output_.P = P;
					// std::cout<<"model P filtered:"<<P<<std::endl;
					UKF_Output_.FilterTime = UKF_Input.meanTime;
					likelihood_ = likelihood;

					output = UKF_Output_;
					likelihood_output = likelihood;
					return true;
				}
			}
		}
		// is_initalized_ = false;
		return false;
	}
	else
	{
		return true;
	}
}

//// 下列函数在UKF_Update中调用
// 生成sigma点
Eigen::MatrixXd UKF::MakeSigmaPoints()
{
	// std::cout<<"p1"<<std::endl;
	// 初始化扩展后的状态向量
	Eigen::VectorXd x_aug = UKF_Output_.State; //Eigen::VectorXd(n_x_);
	// 将原状态向量赋值给扩展状态向量的前n_x_个元素
	// x_aug.head(n_x_) = 
	// std::cout<<"nx:"<<n_x_<<std::endl;
	// std::cout<<"x_aug"<<x_aug<<std::endl;

	// 初始化扩展后的协方差矩阵
	Eigen::MatrixXd P_aug = Eigen::MatrixXd::Zero(n_x_, n_x_);;
	// 将原协方差矩阵赋值给扩展协方差矩阵
	P_aug = UKF_Output_.P;


	// 对扩展协方差矩阵进行Cholesky分解，得到下三角矩阵L
	Eigen::MatrixXd L = P_aug.llt().matrixL();
	// 初始化sigma点矩阵
	Eigen::MatrixXd sigma_points = Eigen::MatrixXd(n_x_, 2 * n_x_ + 1);
	sigma_points.fill(0.0);

	// 将扩展状态向量赋值给sigma点矩阵的第一列
	sigma_points.col(0) = x_aug;
	// 计算常数c
  	const float c = std::sqrt(lambda_ + n_x_);
	for(int i = 0; i < n_x_; ++i) {
		// 计算第i个维度的正方向sigma点
		sigma_points.col(i + 1) = x_aug + c * L.col(i);
		// 计算第i个维度的负方向sigma点
		sigma_points.col(i + n_x_ + 1) = x_aug - c * L.col(i);

		// // 保证z vz az始终为0
		sigma_points(2, i + 1) = 0.0;  // 第3行对应索引为2的行
		sigma_points(2, i + n_x_ + 1) = 0.0;  // 第3行对应索引为2的行
		sigma_points(5, i + 1) = 0.0;  // 第3行对应索引为2的行
		sigma_points(5, i + n_x_ + 1) = 0.0;  // 第3行对应索引为2的行
		sigma_points(8, i + 1) = 0.0;  // 第3行对应索引为2的行
		sigma_points(8, i + n_x_ + 1) = 0.0;  // 第3行对应索引为2的行
	}


	return sigma_points;
}



// 计算时间间隔，更新运动模型
// 其中，若当前时间小于上次时间，使用逆运动模型；若当前测量时间在上次输出结果时间之后，则使用原运动模型
// 若时间间隔为0，抛出异常
bool UKF::UpdateKinematicMatrix_(Eigen::MatrixXd &KinematicMatrix, uint64_t MeasureTime, uint64_t pretime, int model)
{
	uint64_t deltatimeMs;
	float deltatime;
	// uint类型没有负数，需要转换一下
	if((MeasureTime >= pretime))
	{
		deltatimeMs = (MeasureTime - pretime);
		deltatime = ConvertMsToSecond(deltatimeMs);
	}
	else
	{
		deltatimeMs = pretime - MeasureTime;
		deltatime = ConvertMsToSecond(deltatimeMs);
		deltatime = - deltatime;
	}
	// std::cout<<"pretime:"<<int(pretime)<<std::endl;
	// std::cout<<"MeasureTime:"<<int(MeasureTime)<<std::endl;
	// std::cout<<"deltatimeMs:"<<int(deltatimeMs)<<std::endl;
	
	KinematicMatrix = Eigen::MatrixXd::Identity(n_x_, n_x_);

	if (deltatimeMs < DELTA_TIME_FOR_UKF_PROCRSS)
	{
		return true;
	}
	if(n_x_ < 10)
	{
		// 维度无法匹配
		return false;
	}
	
	switch (model)
	{
	case 1:
		{
			GenerateCV(KinematicMatrix, deltatime);
			return true;
		}
		break;

	case 2:
		{
			GenerateCA(KinematicMatrix, deltatime);
			return true;
		}
		break;

	case 3:
		{
			if(abs(UKF_Output_.State(9))<0.000001)
			{
				UKF_Output_.State(9) = 0.000001;
			}
			GenerateCT(KinematicMatrix, deltatime, UKF_Output_.State(9));
			return true;
		}
		break;
	
	default:
		break;
	}
	return false;
}


void UKF::GenerateCA(Eigen::MatrixXd &KinematicMatrix, float deltatime)
{
	float d2 = pow(deltatime, 2);
	for (size_t i = 0; i < 6; i++)
	{
		KinematicMatrix(i, i+3) = deltatime;
	}
	if(deltatime>=0)
	{
		for (size_t i = 0; i < 3; i++)
		{
			KinematicMatrix(i, i+6) = d2 * 0.5;
		}
	}
}

void UKF::GenerateCV(Eigen::MatrixXd &KinematicMatrix, float deltatime)
{
	for (int i = 6; i < 9; i++)
	{
		KinematicMatrix(i,i) = 0.0;
	}
	for (int i = 0; i < 3; i++)
	{
		KinematicMatrix(i,i+3) = deltatime;
	}
}

void UKF::GenerateCT(Eigen::MatrixXd &KinematicMatrix, float deltatime, float w)
{
	float sinValue = sin(deltatime * w);
	float cosValue = cos(deltatime * w);
	float w2 = pow(w, 2);

	KinematicMatrix(3,3) = cosValue;
	KinematicMatrix(3,4) = -sinValue;
	KinematicMatrix(4,3) = sinValue;
	KinematicMatrix(4,4) = cosValue;
	KinematicMatrix(2,5) = deltatime;

	KinematicMatrix(9,9) = 1;

	if(deltatime>=0)
	{
		KinematicMatrix(0,3) = sinValue / w;
		KinematicMatrix(0,4) = (cosValue -1) / w2;
		KinematicMatrix(1,3) = (1 - cosValue) / w2;
		KinematicMatrix(1,4) = sinValue / w;
	}
	else
	{
		KinematicMatrix(0,3) = (2*sin(deltatime*w) - sin(2*deltatime*w) + w*sin(2*deltatime*w))/(2*w2);
		KinematicMatrix(0,4) = ((cos(deltatime*w) - 1)*(w - cos(deltatime*w) + w*cos(deltatime*w)))/w2;
		KinematicMatrix(1,3) = -((cos(deltatime*w) - 1)*(w - cos(deltatime*w) + w*cos(deltatime*w)))/w2;
		KinematicMatrix(1,4) = (2*sin(deltatime*w) - sin(2*deltatime*w) + w*sin(2*deltatime*w))/(2*w2);
	}
}

// 计算sigma点的预测，均值X_hat P_hat
bool UKF::Prediction(Eigen::VectorXd &X_hat,Eigen::MatrixXd &P_hat, Eigen::MatrixXd &sigma_points_pre,
					Eigen::MatrixXd sigma_points, Eigen::MatrixXd KinematicMatrix, 
					int n_x, Eigen::MatrixXd Q, Eigen::VectorXd mean_weights, Eigen::VectorXd cov_weights)
{
	sigma_points_pre = Eigen::MatrixXd(n_x_, 2 * n_x_ + 1);
	sigma_points_pre.fill(0);
	// std::cout<<"sigma_points_:"<<sigma_points_<<std::endl;
    float x,y,z,vx,vy,vz,ax,ay,az,w;
    for (int i = 0; i < sigma_points.cols(); i++)
	{
		x = 0.0;
		y = 0.0;
		z = 0.0;
		vx = 0.0;
		vy = 0.0;
		vz = 0.0;
		ax = 0.0;
		ay = 0.0;
		az = 0.0;
		w = 0.001;
		//int old_w = sigma_points_.col(i)(9);
		
		switch (model_)
		{
		case 1:
			{
				//速度如何估计
				x = sigma_points(0,i)  + sigma_points(3,i) * KinematicMatrix(0,3);
				y = sigma_points(1,i)  + sigma_points(4,i) * KinematicMatrix(1,4);
				z = sigma_points(2,i)  + sigma_points(5,i) * KinematicMatrix(2,5);
				vx = sigma_points(3,i) ;
				vy = sigma_points(4,i) ;
				vz = sigma_points(5,i) ;
				break;
			}
		case 2:
			{
				x = sigma_points(0,i)  + sigma_points(3,i) * KinematicMatrix(0,3) + sigma_points(6,i) * KinematicMatrix(0,6);
				y = sigma_points(1,i)  + sigma_points(4,i) * KinematicMatrix(1,4) + sigma_points(7,i) * KinematicMatrix(1,7);
				z = sigma_points(2,i)  + sigma_points(5,i) * KinematicMatrix(2,5) + sigma_points(8,i) * KinematicMatrix(2,8);
				vx = sigma_points(3,i) 	+ sigma_points(6,i) * KinematicMatrix(3,6);
				vy = sigma_points(4,i) 	+ sigma_points(7,i) * KinematicMatrix(4,7);
				vz = sigma_points(5,i) 	+ sigma_points(8,i) * KinematicMatrix(5,8);
				ax = sigma_points(6,i);
				ay = sigma_points(7,i);
				az = sigma_points(8,i);
				break;
			}
		case 3:
			{
				x = sigma_points(0,i)  + sigma_points(3,i) * KinematicMatrix(0,3) + sigma_points(4,i) * KinematicMatrix(0,4);
				y = sigma_points(1,i)  + sigma_points(3,i) * KinematicMatrix(1,3) + sigma_points(4,i) * KinematicMatrix(1,4);
				z = sigma_points(2,i)  + KinematicMatrix(2,5) * sigma_points(5,i);

				vx = sigma_points(3,i) * KinematicMatrix(3,3) + sigma_points(4,i) * KinematicMatrix(3,4);
				vy = sigma_points(3,i) * KinematicMatrix(4,3)	+ sigma_points(4,i) * KinematicMatrix(4,4);
				vz = sigma_points(5,i);

				ax = sigma_points(6,i);
				ay = sigma_points(7,i);
				az = sigma_points(8,i);

				w = sigma_points(9,i);// * prediction_Matrix(9,9); // 1
				break;
			}
		default:
			try
			{
				throw std::exception();
			}
			catch(const std::exception& e)
			{
				std::cerr << e.what() << '\n';
			}
			
			break;
		}
		sigma_points_pre(0,i) = x;
		sigma_points_pre(1,i) = y;
		sigma_points_pre(2,i) = 0.0;//z;
		sigma_points_pre(3,i) = vx;
		sigma_points_pre(4,i) = vy;
		sigma_points_pre(5,i) = 0.0;//vz;
		sigma_points_pre(6,i) = ax;
		sigma_points_pre(7,i) = ay;
		sigma_points_pre(8,i) = 0.0;//az;
		sigma_points_pre(9,i) = w;
	}

	
	X_hat = Eigen::VectorXd(n_x);
	P_hat = Eigen::MatrixXd(n_x,n_x);
	//计算预测均值
	X_hat.fill(0.0);
	// std::cout << "sigma_points_pre dimensions: " << sigma_points_pre.rows() << "x" << sigma_points_pre.cols() << std::endl;

	// std::cout<<"sigma_points_pre:"<<sigma_points_pre<<std::endl;
	for(int i=0; i<2*n_x+1; i++){
		// std::cout<<"mean_weights("<<i<<"):"<<mean_weights(i)-int(mean_weights(i))<<std::endl;
		// std::cout<<"sigma_points_pre.col("<<i<<"):"<<sigma_points_pre.col(i)<<std::endl;
		// std::cout<<"X_hat:"<<X_hat<<std::endl;
		auto temp = mean_weights(i)*sigma_points_pre.col(i);
		X_hat += temp;
		// std::cout<<"mean_weights(i)*sigma_points_pre.col(i):"<<mean_weights(i)*sigma_points_pre.col(i)(1) - int(mean_weights(i)*sigma_points_pre.col(i)(1))<<std::endl;
		// std::cout<<"X_hat1:"<<X_hat(1)-int(X_hat(1))<<std::endl;
	}

	// X_hat(2) = 0.0;
	// X_hat(5) = 0.0;
	// X_hat(8) = 0.0;

	std::cout<<"next state predicted:"<<X_hat<<std::endl;
	//std::cout<<"########## 计算预测均值x_ ##########"<<'\n'<<x_<<std::endl;
	//计算预测协方差
	try{
		P_hat.fill(0.0);
		for(int i=0; i<2*n_x+1; i++){
			Eigen::VectorXd x_diff = sigma_points_pre.col(i)-X_hat;

			x_diff *= x_diff_gamma_;

			// x_diff(3) =  normalangle(x_diff(3));//,x_diff(2));
			P_hat += cov_weights(i) * x_diff * x_diff.transpose();
			// std::cout<<"x_diff:"<<x_diff<<std::endl;
			// std::cout<<"P_hat:"<<P_hat<<std::endl;
		}
      		
		P_hat += Q;
		// std::cout<<"计算预测协方差P_ "<<'\n'<<P_hat<<std::endl;
	}
	catch (const std::bad_alloc& e){
		std::cout<<"erro"<<std::endl;
		return false;
	}
	return true;

}


// 由多个sigma点和实际测量计算的观测预测 Zk, sigma预测zk1temp
bool UKF::PredictionZ_hat(Eigen::VectorXd &mean_Measurements_hat, Eigen::MatrixXd &Sigma_Measurements_hat,
                          Eigen::VectorXd mean_weights, Eigen::MatrixXd sigma_points_pre,
                          std::vector<Eigen::MatrixXd> MeasurementsMatrix, std::vector<double> Z) {
    
	int mlen = MeasurementsMatrix.size();
	
    if (mlen == 0) {
        std::cerr << "Error: Invalid input dimensions for MeasurementsMatrix." << std::endl;
        return false;
    }
	if(sigma_points_pre.cols() != 2 * n_x_ + 1){
		std::cerr << "Error: Invalid input dimensions for sigma_points_pre." << std::endl;
        return false;
	}

    // 初始化 Sigma_Measurements_hat 矩阵
    Sigma_Measurements_hat = Eigen::MatrixXd(mlen * 2, 2 * n_x_ + 1);
    Sigma_Measurements_hat.setZero();

    Eigen::VectorXd Measurement(3);
    Eigen::VectorXd XYZ_N(4);
    XYZ_N[3] = 1.0; // 固定第四维为 1

    // 遍历所有 sigma 点
    for (int i = 0; i < 2 * n_x_ + 1; ++i) {
        // 遍历所有的测量矩阵
        for (int j = 0; j < mlen; ++j) {
            Eigen::MatrixXd &measurementMatrix = MeasurementsMatrix[j];
			auto &z = Z[j];
			// std::cout << "measurementMatrix:"<<measurementMatrix<< std::endl;
            if (measurementMatrix.cols() != 4 || measurementMatrix.rows() != 3) {
                std::cerr << "Error: Invalid measurementMatrix dimensions at index " << j << "." << std::endl;
                return false;
            }

			// sigma_points_pre(2, i) = 0.0;
            XYZ_N.head(3) = sigma_points_pre.col(i).head(3); // 提取前三维
            Measurement = measurementMatrix * XYZ_N;//z - sigma_points_pre(2, i)

			// 避免 sigma_points_pre_(2, i) 过小导致除以零
			// if (std::abs((z - sigma_points_pre(2, i))) < 1e-30) {
			if (std::abs(Measurement(2)) < 1e-30) {
				return false;
				// sigma_points_pre(2, i) = (sigma_points_pre(2, i) > 0 ? 1e-10 : -1e-10);
			}

			// Measurement = 1/(z - sigma_points_pre(2, i))*Measurement;// / Measurement(2);
			Measurement = 1 / Measurement(2) * Measurement;

            // 更新 Sigma_Measurements_hat
            Sigma_Measurements_hat(j * 2, i) = Measurement(0);
            Sigma_Measurements_hat(j * 2 + 1, i) = Measurement(1);
        }
    }

    // 初始化 mean_Measurements_hat
    mean_Measurements_hat = Eigen::VectorXd::Zero(mlen * 2);

	// std::cout<<"mean_weights"<<mean_weights<<std::endl;
	// std::cout<<"Sigma_Measurements_hat"<<Sigma_Measurements_hat<<std::endl;
    // 计算预测的加权均值
    for (int i = 0; i < 2 * n_x_ + 1; ++i) {
		// std::cout<<"mean_weights("<<i<<")"<<mean_weights(i)<<std::endl;
        mean_Measurements_hat += mean_weights(i) * Sigma_Measurements_hat.col(i);
		// std::cout<<"Sigma_Measurements_hat.col("<<i<<")"<<Sigma_Measurements_hat.col(i)<<std::endl;
		// std::cout<<"mean_Measurements_hat"<<mean_Measurements_hat<<std::endl;
    }


    return true;
}

// bool UKF::PredictionZ_hat(Eigen::VectorXd &mean_Measurements_hat, Eigen::MatrixXd &Sigma_Measurements_hat,
// 					Eigen::VectorXd mean_weights, Eigen::MatrixXd sigma_points_pre,
// 					std::vector<Eigen::MatrixXd> MeasurementsMatrix)
// {
// 	int mlen = MeasurementsMatrix.size();
// 	// 初始化Z_sigma_矩阵，大小为n_z_行，2*n_x_+1列，并填充为0
// 	Sigma_Measurements_hat = Eigen::MatrixXd(mlen * 2, 2 * n_x_ + 1);
// 	Sigma_Measurements_hat.fill(0.0);
//     Eigen::VectorXd Measurement(3);
// 	// 遍历所有sigma点，利用对应的MeasurementsMatrix生成量测数据
// 	for (int i = 0; i < 2 * n_x_ + 1; ++i)
// 	{
// 		if(abs(sigma_points_pre(2,i))<0.00001)
// 		{
// 			sigma_points_pre(2,i) = 0.00001;
// 		}

// 		for(int j = 0; j < mlen ; j++)
// 			//const auto &measurementMatrix : UKF_Input_.MeasurementsMatrix)
// 		{
// 			Eigen::MatrixXd measurementMatrix = MeasurementsMatrix[j];
// 			std::cout<<"measurementMatrix:"<<measurementMatrix<<std::endl;
// 			Eigen::VectorXd XYZ_N(4);
// 			XYZ_N.topRows(3) = sigma_points_pre.col(i).topRows(3);
// 			XYZ_N.row(3).setConstant(1.0);
// 			Measurement = 1.0 / sigma_points_pre_(2,i) * measurementMatrix * XYZ_N;
// 			Sigma_Measurements_hat(j*2,i) = Measurement(0);
// 			Sigma_Measurements_hat(j*2 + 1,i) = Measurement(1);
// 		}
		
// 		// std::cout<<"Sigma_Measurements_hat.col(i):"<<Sigma_Measurements_hat.col(i)<<std::endl;

// 	}
// 	std::cout<<"Sigma_Measurements_hat:"<<Sigma_Measurements_hat<<std::endl;
// 	// 初始化Zk1预测测量值均值 z_pre_向量，大小为n_z_，并填充为0
// 	mean_Measurements_hat = Eigen::VectorXd(mlen*2);
// 	mean_Measurements_hat.fill(0.0);

// 	// 计算加权后的预测均值
// 	for (int i = 0; i < 2 * n_x_ + 1; ++i) {	
// 		mean_Measurements_hat += mean_weights(i) * Sigma_Measurements_hat.col(i);
// 	}
// 	// std::cout<<"mean_weights:\n"<<mean_weights<<std::endl;
// 	// std::cout<<"mean_Measurements_hat:\n"<<mean_Measurements_hat<<std::endl;
// 	return true;
// }

// 量测更新：根据合并的R，所有sigma的观测预测向量，和Wc，预测协方差S和T，
// 用S T Zk 输入的实际观测MeasurementsVector，计算该模型的X1 P1, （卡尔曼增益）
//  输入Zk， sigma观测预测，实际观测， R， Wc， 
bool UKF::UpdateState(Eigen::VectorXd &State,Eigen::MatrixXd &P, Eigen::MatrixXd &S, Eigen::VectorXd &Zminus, double &likelihood,
				Eigen::MatrixXd Sigma_Measurements_hat, Eigen::VectorXd mean_Measurements_hat,
				Eigen::VectorXd X_hat, Eigen::MatrixXd P_hat,
				Eigen::MatrixXd sigma_points_pre,
				Eigen::VectorXd MeasurementsVector,
				int n_x, 
				Eigen::MatrixXd R_merge, Eigen::VectorXd cov_Weights)
{
	int Zlen = UKF_Input_.MeasurementsMatrix.size() * 2;
	// std::cout<<"Zlen:"<<Zlen<<std::endl;
	// 初始化预测方差(Pzz) S_矩阵，并填充为0
	S = Eigen::MatrixXd(Zlen,Zlen);

	S.fill(0.0);

	
	std::cout<<"mean_Measurements_hat:\n"<<mean_Measurements_hat<<std::endl;
	// std::cout<<"cov_Weights:\n"<<cov_Weights<<std::endl;
	Eigen::VectorXd z_diff;
	z_diff.fill(0.0);
	// 计算加权后的预测协方差S_
	for(int i=0; i < (2*n_x_+ 1); i++){
		
		z_diff = Sigma_Measurements_hat.col(i) - mean_Measurements_hat;

		z_diff *= z_diff_gamma_;
		// std::cout<<"z_diff:\n"<<z_diff<<std::endl;
		// std::cout<<"cov_Weights("<<i<<"):\n"<<cov_Weights(i)<<std::endl;
		S += (cov_Weights(i) * z_diff * z_diff.transpose());
		// std::cout<<"S:\n"<<S<<std::endl;
	}
	z_diff.fill(0.0);
	// 加上观测噪声协方差R_
	S += R_merge;
	// std::cout<<"S:\n"<<S<<std::endl;
	// 创建一个n_x_行n_z_列的零矩阵T
	Eigen::MatrixXd T = Eigen::MatrixXd(n_x,Zlen);
	T.fill(0.0);
	for(int i=0; i<(2*n_x+1); i++){
		// 计算第i个sigma点与均值x_的差值
		Eigen::VectorXd x_diff = sigma_points_pre.col(i)-X_hat;
		x_diff *= x_diff_gamma_;
		// 将x_diff的第4个元素进行规范化处理，这里注释掉了原代码中的处理方式
		// x_diff(3) =  normalangle(x_diff(3));//,x_diff(2));
		// 计算第i个Z_sigma点与预测值z_pre_的差值
		z_diff = Sigma_Measurements_hat.col(i) - mean_Measurements_hat;
		z_diff *= z_diff_gamma_;
		// 根据权重和差值计算T矩阵
		T += cov_Weights(i) * x_diff * z_diff.transpose();
	}
	// std::cout<<"T:\n"<<T<<std::endl;

	// 卡曼增益计算
	// 创建一个n_x_行n_z_列的零矩阵K
	//卡曼增益
	Eigen::MatrixXd K = Eigen::MatrixXd(n_x,Zlen);
	
	// 使用T矩阵和S_矩阵的逆矩阵计算K矩阵
	K = T * S.inverse();

	// std::cout<<"K:\n"<<K<<std::endl;

	std::cout<<"MeasurementsVector:\n"<<MeasurementsVector<<std::endl;

	// JPDAF更新
	// 创建一个n_z_维的零向量Zminus_
	//JPDAF update
	Zminus = Eigen::VectorXd(Zlen);
	Zminus.fill(0.0);
	// 计算Z与预测值z_pre_的差值
	Zminus = (MeasurementsVector - mean_Measurements_hat);
	std::cout<<"Zminus:\n"<<Zminus<<std::endl;
	// 更新状态向量x_   X_hat
	// std::cout<<"X_hat:\n"<<X_hat<<std::endl;
	

	// if(AddTemp(0) > 50.0)
	// {
	// 	AddTemp(0) = 50.0;
	// }
	// else if(AddTemp(0) < -50.0)
	// {
	// 	AddTemp(0) = -50.0;
	// }

	// if(AddTemp(1) >50.0)
	// {
	// 	AddTemp(1) = 50.0;
	// }
	// else if(AddTemp(1) < -50.0)
	// {
	// 	AddTemp(1) = -50.0;
	// }
	if(!K.hasNaN())
	{
		K_ = K;

		auto AddTemp = K * Zminus;
		P = P_hat- K * S * K.transpose();

		// double norm = sqrt(AddTemp(0)*AddTemp(0)+AddTemp(1)*AddTemp(1));
		// if(norm>5)
		// {
		// 	AddTemp*=5/norm;
		// }
		State = X_hat + AddTemp * change_gamma_;

		if(X_hat.hasNaN())
		{
			// std::cout
		}

	}
	else{
		K = K_;
	}
	
	// State = UKF_Output_.State + K * Zminus;

	// std::cout<<"State:\n"<<State<<std::endl;
	
	// std::cout<<"P_hat:"<<P_hat<<std::endl;
	// std::cout<<"K * S * K.transpose():"<<K * S * K.transpose()<<std::endl;
	// 更新协方差矩阵P_
	
	// P = UKF_Output_.P - K * S * K.transpose();
	// std::cout<<"update P:"<<P<<std::endl;

	likelihood = Updatelikelihood(Zminus, S);
	// likelihood_ = likelihood;
	std::cout<<"likelihood:"<<likelihood<<std::endl;


	if(std::isnan(likelihood) || likelihood == 0)
	{
		// return false;
		P = default_P;
		likelihood = 1e-30;

	}

	if(State.hasNaN())
		return false;


	// if(likelihood == 0)
	// {
	// 	likelihood = 1e-30;
	// }
		
	return true;
}

// // 只有最后一次更新需要更新和输出S，Zminus，和 likelihood
// bool UKF::UpdateState(Eigen::VectorXd &State,Eigen::MatrixXd &P,Eigen::MatrixXd &S, Eigen::VectorXd &Zminus, 
// 				Eigen::MatrixXd Sigma_Measurements_hat, Eigen::VectorXd mean_Measurements_hat,
// 				Eigen::VectorXd X_hat, Eigen::MatrixXd P_hat,
// 				Eigen::MatrixXd sigma_points_pre,
// 				Eigen::VectorXd MeasurementsVector,
// 				int n_x, 
// 				Eigen::MatrixXd R_merge, Eigen::VectorXd cov_Weights)
// {
	// int Zlen = UKF_Input_.MeasurementsMatrix.size() * 2;

	// // 初始化预测方差(Pzz) S_矩阵，并填充为0
	// S = Eigen::MatrixXd(Zlen,Zlen);

	// S.fill(0.0);

	// Eigen::VectorXd z_diff;
	// // 计算加权后的预测协方差S_
	// for(int i=0; i < 2*n_x + 1; ++i){
	// 	z_diff = Sigma_Measurements_hat.col(i) - mean_Measurements_hat;
	// 	S += (cov_Weights(i) * z_diff * z_diff.transpose());
	// }

	// // 加上观测噪声协方差R_
	// S += R_merge;

	// // 创建一个n_x_行n_z_列的零矩阵T
	// Eigen::MatrixXd T = Eigen::MatrixXd(n_x,Zlen);
	// T.fill(0.0);
	// for(int i=0; i<2*n_x+1; ++i){
	// 	// 计算第i个sigma点与均值x_的差值
	// 	Eigen::VectorXd x_diff = sigma_points_pre.col(i)-X_hat;
	// 	// 将x_diff的第4个元素进行规范化处理，这里注释掉了原代码中的处理方式
	// 	// x_diff(3) =  normalangle(x_diff(3));//,x_diff(2));
	// 	// 计算第i个Z_sigma点与预测值z_pre_的差值
	// 	z_diff = Sigma_Measurements_hat.col(i) - mean_Measurements_hat;
	// 	// 根据权重和差值计算T矩阵
	// 	T += cov_Weights(i) * x_diff * z_diff.transpose();
	// }

	// // 卡曼增益计算
	// // 创建一个n_x_行n_z_列的零矩阵K
	// //卡曼增益
	// Eigen::MatrixXd K = Eigen::MatrixXd(n_x,Zlen);
	// // 使用T矩阵和S_矩阵的逆矩阵计算K矩阵
	// K = T * S.inverse();

	// // JPDAF更新
	// // 创建一个n_z_维的零向量Zminus_
	// //JPDAF update
	// Zminus = Eigen::VectorXd(Zlen);
	// Zminus.fill(0.0);
	// // 计算Z与预测值z_pre_的差值
	// Zminus = (MeasurementsVector - mean_Measurements_hat);

	// // 更新状态向量x_   X_hat
	// State = X_hat + K * Zminus;//UKF_Output_.State + K * Zminus;

	// // 更新协方差矩阵P_
	// P = P_hat- K * S * K.transpose();//UKF_Output_.P - K * S * K.transpose();

// 	return false;
// }

double UKF::Updatelikelihood(Eigen::VectorXd &Zminus, Eigen::MatrixXd &S){

	double pi_ = 3.141592653589793;
	// 计算每个模型的权重ita
	double ita = 1.0/(sqrt(2.0*pi_)*sqrt(S.determinant()))*exp(-0.5* Zminus.transpose() * S.inverse() * Zminus);
	if(ita < 1e-30)
		return 1e-30;
	else
		return ita;
}

// 定义里可以不写默认参数
void UKF::GenerateSigmaWeights_(Eigen::VectorXd &mean_weights, Eigen::VectorXd &cov_weights, double &lambda, double n_x, double alpha, double beta)
{
	lambda = ((alpha*alpha)-1.0)*n_x;

	//权重
	cov_weights = Eigen::VectorXd(2 * n_x_ + 1);
	mean_weights = Eigen::VectorXd(2 * n_x_ + 1);
	cov_weights(0) = lambda/(n_x+lambda)+(1.-(alpha*alpha)+beta);
	mean_weights(0) = (lambda/(lambda+n_x));
	double w = 0.5/(lambda+n_x);
	for(int i =1; i<2*n_x_+1; ++i){
		cov_weights(i) = (w);
		mean_weights(i) = (w);
	}
}

